//
// Created by bourne on 2024/6/19.
//

#ifndef SRC_FAST_EXPLORATION_MANAGER_H
#define SRC_FAST_EXPLORATION_MANAGER_H

#include <ros/ros.h>
#include <Eigen/Eigen>
#include <memory>
#include <vector>

using Eigen::Vector3d;
using std::pair;
using std::shared_ptr;
using std::tuple;
using std::unique_ptr;
using std::vector;

namespace fast_planner {

// 定义了与探索任务相关的类和结构
class EDTEnvironment;
class SDFMap;
class FastPlannerManager;
// class UniformGrid;
class HGrid;
class FrontierFinder;
struct ExplorationParam;
struct ExplorationData;

// 定义探索结果的枚举类型
enum EXPL_RESULT { NO_FRONTIER, FAIL, SUCCEED, NO_GRID };

class FastExplorationManager {
public:
  FastExplorationManager();
  ~FastExplorationManager();

  void initialize(ros::NodeHandle& nh);

  // 规划探索运动的函数
  // 调用探索管理器的planExploreMotion函数来执行完整的探索规划
  int planExploreMotion(
      const Vector3d& pos, const Vector3d& vel, const Vector3d& acc, const Vector3d& yaw);

  // 规划到视点的轨迹函数
  // 这个函数用于在避障(由于collision触发或者集群轨迹collision触发的时候)或返回起始点时规划轨迹
  int planTrajToView(const Vector3d& pos, const Vector3d& vel, const Vector3d& acc,
                     const Vector3d& yaw, const Vector3d& next_pos, const double& next_yaw);

  // 更新frontier结构的函数
  int updateFrontierStruct(const Eigen::Vector3d& pos);

  // 分配网格给无人机的函数
  void allocateGrids(const vector<Eigen::Vector3d>& positions,
                     const vector<Eigen::Vector3d>& velocities, const vector<vector<int>>& first_ids,
                     const vector<vector<int>>& second_ids, const vector<int>& grid_ids, vector<int>& ego_ids,
                     vector<int>& other_ids);

  // Find optimal tour visiting unknown grid
  // 查找访问未知网格的最优巡游路径的函数
  bool findGlobalTourOfGrid(const vector<Eigen::Vector3d>& positions,
                            const vector<Eigen::Vector3d>& velocities, vector<int>& ids, vector<vector<int>>& others,
                            bool init = false);

  // 计算网格路径成本的函数
  double computeGridPathCost(const Eigen::Vector3d& pos, const vector<int>& grid_ids,
                             const vector<int>& first, const vector<vector<int>>& firsts,
                             const vector<vector<int>>& seconds, const double& w_f);

  // not used here
  void calcMutualCosts(const Eigen::Vector3d& pos, const double& yaw, const Eigen::Vector3d& vel,
                       const vector<pair<Eigen::Vector3d, double>>& views, vector<float>& costs);

  shared_ptr<ExplorationData> ed_;
  shared_ptr<ExplorationParam> ep_;
  shared_ptr<FastPlannerManager> planner_manager_;
  shared_ptr<FrontierFinder> frontier_finder_;
  shared_ptr<HGrid> hgrid_;
  shared_ptr<SDFMap> sdf_map_;
  // shared_ptr<UniformGrid> uniform_grid_;
private:
  shared_ptr<EDTEnvironment> edt_environment_;
  ros::ServiceClient tsp_client_, acvrp_client_;

  // Find optimal tour for coarse viewpoints of all frontiers
  // 查找所有前沿的粗略视点的最优巡游路径的函数
  void findGlobalTour(const Vector3d& cur_pos, const Vector3d& cur_vel, const Vector3d cur_yaw,
                      vector<int>& indices);

  // Find optimal coordinated tour for quadrotor swarm
  // 查找群体的网格和前沿路径的函数
  void findGridAndFrontierPath(const Vector3d& cur_pos, const Vector3d& cur_vel,
                               const Vector3d& cur_yaw, vector<int>& grid_ids, vector<int>& ftr_ids);

  // Refine local tour for next few frontiers, using more diverse viewpoints
  // 使用更多样化的视点细化局部巡游路径的函数
  void refineLocalTour(const Vector3d& cur_pos, const Vector3d& cur_vel, const Vector3d& cur_yaw,
                       const vector<vector<Vector3d>>& n_points, const vector<vector<double>>& n_yaws,
                       vector<Vector3d>& refined_pts, vector<double>& refined_yaws);

  void shortenPath(vector<Vector3d>& path);

  // 查找前沿的最优巡游路径的函数
  void findTourOfFrontier(const Vector3d& cur_pos, const Vector3d& cur_vel, const Vector3d& cur_yaw,
                          const vector<int>& ftr_ids, const vector<Eigen::Vector3d>& grid_pos, vector<int>& ids);
public:
  typedef shared_ptr<FastExplorationManager> Ptr;
};


}
#endif //SRC_FAST_EXPLORATION_MANAGER_H
